#!/usr/bin/env python
#coding=utf-8

import rospy
import sys, tf, actionlib, moveit_commander
from gazebo_msgs.srv import *
from tf.transformations import quaternion_from_euler
from geometry_msgs.msg import *
import fetch_prepare
import fetch_teleop_key

if __name__ == "__main__":
    rospy.init_node('start_game')

    print("-"*20 + "Game is begining" + "-"*20)
    rospy.wait_for_service('/gazebo/set_model_state')
    get_state_service = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState) # 获取模型位置服务
    set_state_service = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) # 设置模型位置服务

    # 重置小车, 物块 和 fetch
    objstate = SetModelStateRequest()
    # 重置小车
    print("-"*20 + "Reset the car" + "-"*20)
    objstate = SetModelStateRequest()
    objstate.model_state.model_name = "racecar"
    objstate.model_state.pose.position.x = -0.3
    objstate.model_state.pose.position.y = -0.3
    objstate.model_state.pose.position.z = 0
    objstate.model_state.pose.orientation.w = 0.707
    objstate.model_state.pose.orientation.x = 0
    objstate.model_state.pose.orientation.y = 0
    objstate.model_state.pose.orientation.z = 0.707
    objstate.model_state.twist.linear.x = 0
    objstate.model_state.twist.linear.y = 0
    objstate.model_state.twist.linear.z = 0
    objstate.model_state.twist.angular.x = 0
    objstate.model_state.twist.angular.y = 0
    objstate.model_state.twist.angular.z = 0
    objstate.model_state.reference_frame = "world"
    result = set_state_service(objstate)
    # 重置物块
    print("-"*20 + "Reset the objects" + "-"*20)
    ModelNames = ["product_0_clone", "product_0_clone_0", "product_0_yellow", "product_0_clone_1", "product_0_clone_2"]
    posx = 10.025
    posy = 5.345
    posz = 1.024940
    for i in ModelNames:
        objstate = SetModelStateRequest()  # Create an object of type SetModelStateRequest
        posx -= 0.128
        objstate.model_state.model_name = i
        objstate.model_state.pose.position.x = posx
        objstate.model_state.pose.position.y = posy
        objstate.model_state.pose.position.z = posz
        objstate.model_state.pose.orientation.w = 1
        objstate.model_state.pose.orientation.x = 0
        objstate.model_state.pose.orientation.y = 0
        objstate.model_state.pose.orientation.z = 0
        objstate.model_state.twist.linear.x = 0.0
        objstate.model_state.twist.linear.y = 0.0
        objstate.model_state.twist.linear.z = 0.0
        objstate.model_state.twist.angular.x = 0.0
        objstate.model_state.twist.angular.y = 0.0
        objstate.model_state.twist.angular.z = 1.57
        objstate.model_state.reference_frame = "world"
        result = set_state_service(objstate)
    # 重置fetch
    print("-"*20 + "Reset the fetch" + "-"*20)
    fetch_prepare.main()
    objstate = SetModelStateRequest()
    objstate.model_state.model_name = "fetch"
    objstate.model_state.pose.position.x = 5.8
    objstate.model_state.pose.position.y = 4.593
    objstate.model_state.pose.position.z = 0
    objstate.model_state.pose.orientation.w = 0.707
    objstate.model_state.pose.orientation.x = 0
    objstate.model_state.pose.orientation.y = 0
    objstate.model_state.pose.orientation.z = 0.707
    objstate.model_state.twist.linear.x = 0
    objstate.model_state.twist.linear.y = 0
    objstate.model_state.twist.linear.z = 0
    objstate.model_state.twist.angular.x = 0
    objstate.model_state.twist.angular.y = 0
    objstate.model_state.twist.angular.z = 0
    objstate.model_state.reference_frame = "world"
    result = set_state_service(objstate)
    fetch_teleop_key.fetch_stay()        # 使fetch静止, 要不会顺时针转动

    # 获取小车位置并判断是否开启传送带
    RaceCar = GetModelStateRequest()
    RaceCar.model_name = "racecar"
    while True:
        CarState = get_state_service(RaceCar)
        CarPosition = (CarState.pose.position.x, CarState.pose.position.y, CarState.pose.position.z)
        fetch_teleop_key.fetch_stay()
        if (CarPosition[0]<11.5 and CarPosition[0]>11.28 and CarPosition[1]>5.75 and CarPosition[1]<5.85): # 第一目标位置
            break

    # 开启传送带
    print("-"*20 + "Start the conveyor belt" + "-"*20)
    Models = []
    ModelNames = ["product_0_yellow", "product_0_clone", "product_0_clone_0", "product_0_clone_1", "product_0_clone_2"]
    for i in range(5):
        Models.append(GetModelStateRequest())
        Models[i].model_name = ModelNames[i]
    # 再次设置fetch位置
    objstate = SetModelStateRequest()
    objstate.model_state.model_name = "fetch"
    objstate.model_state.pose.position.x = 5.8
    objstate.model_state.pose.position.y = 4.593
    objstate.model_state.pose.position.z = 0
    objstate.model_state.pose.orientation.w = 0.707
    objstate.model_state.pose.orientation.x = 0
    objstate.model_state.pose.orientation.y = 0
    objstate.model_state.pose.orientation.z = 0.707
    objstate.model_state.twist.linear.x = 0
    objstate.model_state.twist.linear.y = 0
    objstate.model_state.twist.linear.z = 0
    objstate.model_state.twist.angular.x = 0
    objstate.model_state.twist.angular.y = 0
    objstate.model_state.twist.angular.z = 0
    objstate.model_state.reference_frame = "world"
    result = set_state_service(objstate)
    fetch_teleop_key.fetch_stay()   # 使fetch静止, 要不会顺时针转动
    # 开始移动物块
    while True:
        # 挑出在传送带上的物块
        ModelsOnConveyor = []
        objstate = []
        state = []
        for i in range(5):
            objstate.append(get_state_service(Models[i]))
            state.append((objstate[-1].pose.position.x, objstate[-1].pose.position.y, objstate[-1].pose.position.z))
            ModelsOnConveyor.append(ModelNames[i])
            if state[-1][0] < 10.15 and state[-1][0] > 1.30 and state[-1][1] > 5.3445 and state[-1][1] < 5.3455 and state[-1][2] < 1.03:
                pass
            else:
                del ModelsOnConveyor[-1]
                del state[-1]
        # 移动在传送带上的物块
        for i,j in zip(ModelsOnConveyor, state):
            posx,posy,posz = j
            posx-=0.005
            objstate = SetModelStateRequest()
            objstate.model_state.model_name = i
            objstate.model_state.pose.position.x = posx
            objstate.model_state.pose.position.y = posy
            objstate.model_state.pose.position.z = posz
            objstate.model_state.pose.orientation.w = 1
            objstate.model_state.pose.orientation.x = 0
            objstate.model_state.pose.orientation.y = 0
            objstate.model_state.pose.orientation.z = 0
            objstate.model_state.twist.linear.x = 0.0
            objstate.model_state.twist.linear.y = 0.0
            objstate.model_state.twist.linear.z = 0.0
            objstate.model_state.twist.angular.x = 0.0
            objstate.model_state.twist.angular.y = 0.0
            objstate.model_state.twist.angular.z = 0.0
            objstate.model_state.reference_frame = "world"
            result = set_state_service(objstate)
            # print(i, j)
            rospy.sleep(0.01/len(state))
        
        # 物块到达终点 或 小车载物块到达终点时 停止传送带
        RaceCar = GetModelStateRequest()
        Yellow = GetModelStateRequest()
        RaceCar.model_name = "racecar"
        Yellow.model_name = "product_0_yellow"
        CarState = get_state_service(RaceCar)
        YellowState = get_state_service(Yellow)
        CarPosition = (CarState.pose.position.x, CarState.pose.position.y, CarState.pose.position.z)
        YellowPosition = (YellowState.pose.position.x, YellowState.pose.position.y, YellowState.pose.position.z)
        if CarPosition[0]<0.13 and CarPosition[0]>-0.07 and CarPosition[1]>5.69 and CarPosition[1]<5.8 \
            and YellowPosition[0]<0.3 and YellowPosition[0]>-0.24 and YellowPosition[1]>5.473 and YellowPosition[1]<6.02: # 终点
            print("Competition complete!")
            break
        if state[-1][0] < 1.40:
            break

    while True:
        RaceCar = GetModelStateRequest()
        Yellow = GetModelStateRequest()
        RaceCar.model_name = "racecar"
        Yellow.model_name = "product_0_yellow"
        CarState = get_state_service(RaceCar)
        YellowState = get_state_service(Yellow)
        CarPosition = (CarState.pose.position.x, CarState.pose.position.y, CarState.pose.position.z)
        YellowPosition = (YellowState.pose.position.x, YellowState.pose.position.y, YellowState.pose.position.z)
        if CarPosition[0]<0.13 and CarPosition[0]>-0.07 and CarPosition[1]>5.68 and CarPosition[1]<5.8 \
            and YellowPosition[0]<0.3 and YellowPosition[0]>-0.24 and YellowPosition[1]>5.473 and YellowPosition[1]<6.02: # 终点
            print("Competition complete!")
            break
